/****************************************************************************
 *
 *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

#include "serial_node.hpp"

SerialNode::SerialNode() :
	ModuleParams(nullptr),
	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::test1)
{
}

SerialNode::~SerialNode()
{
	perf_free(_loop_perf);
	perf_free(_loop_interval_perf);
}

bool SerialNode::init()
{

    //设置程序运行频率
ScheduleOnInterval(500_ms);
        return true;
}

//int SerialNode::depth_parse(char c, char *parserbuf, unsigned *parserbuf_index, float *depth)
//{
//if(c=='F')
//{
//    parserbuf[]
//}
//}

int SerialNode::collect()
{
perf_begin(_loop_perf);
//char readbuf[sizeof(_linebuf)] {};
//unsigned readlen = sizeof(readbuf);
// float depth = -1.0f;
//int ret = 0;

//// Check the number of bytes available in the buffer
int bytes_available = 0;
::ioctl(_fd, FIONREAD, (unsigned long)&bytes_available);

if (!bytes_available) {
       perf_end(_loop_perf);
       return 0;
}
// PX4_INFO("bytes_available=%d\n",bytes_available);
mavlink_log_info(&_mavlink_log_pub, "bytes_available:%d",bytes_available);
//do {
//        // read from the sensor (uart buffer)
//        ret = ::read(_fd, &readbuf[0], readlen);

//        if (ret < 0) {
//                PX4_ERR("read err: %d", ret);
//                perf_end(_loop_perf);
//        }


//        // parse buffer
//        for (int i = 0; i < ret; i++) {
//                depth_parse(readbuf[i], _linebuf, &_linebuf_index, &depth);
//        }

//        // bytes left to parse
//        bytes_available -= ret;

//} while (bytes_available > 0);







    ret = ::read(_fd, &data, 1);
    if (ret < 0)
    {
        PX4_ERR("read err: %d", ret);
    }
    else
    {
        if(data == 'F')
        {
            read(_fd, &data, 1);
            if(data == 'e')
            {
                read(_fd, &data, 1);
              if(data == 't')
              {
                  read(_fd, &data, 1);
                   if((data == ' ')||(data == 'E')||(data == 'O'))
                   {
                       read(_fd, &data, 1);
                       if(data == 'H')
                       {
                           read(_fd, &data, 1);
                            if(data == ' ')
                            {
                                for(int i = 0; i <5; ++i)
                                {
                                    read(_fd,&data,1);
                                    readbuf[i] = data;
                                    data = '0';
                    //             printf("readbuf[%d]:%c\n",i,readbuf[i]);
                                }
                            }
                       }
                   }
              }
            }




//            for(int i = 0; i <10; ++i)
//            {
//                read(_fd,&data,1);
//                readbuf[i] = data;
//                data = '0';
////             printf("readbuf[%d]:%c\n",i,readbuf[i]);
//            }
//            PX4_INFO("readbuf:%s\n",readbuf);
            result_read=(double)(readbuf[0]-'0')*100+(double)(readbuf[1]-'0')*10+(double)(readbuf[2]-'0')+(double)(readbuf[3]-'0')*0.1+(double)(readbuf[4]-'0')*0.01;
            if(abs(result_read-result_last)>100)
            {
            result=result_last;
            }
            else
            {
            result=result_read;
            }
            printf("result:%lf\n",result);
            send_data.altitude=result;

            send_data.timestamp=hrt_absolute_time();
            _send_data_pub.publish(send_data);
            result_last=result_read;
//        senser_data.timestamp=hrt_absolute_time();
//        senser_data.max_distance=result;
//         _senser_data_pub.publish(senser_data);
        }

    }
    ret = ::write(_fd,&readbuf,10);
    if (ret < 0)
    {
        PX4_ERR("write err: %d", ret);
    }
    else
    {
        //  PX4_INFO("write buf:%f",result);
    }
//    memset(&readbuf,0,sizeof(char[10]));
 perf_end(_loop_perf);
    return PX4_OK;

}


void SerialNode::Run()
{


        // open fd
        //pixhawk4 uart&IIC ttyS3
        //cuav nora+ X7+ telem2 ttyS3
    //cuav X7+ uart4 ttyS2
    //	cuav X7+	GPS1:/dev/ttyS0
    //	cuav X7+	TEL1:/dev/ttyS1


//V6C
// CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0"
// CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS6"
// CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS5"
// CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3"
// CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1"
// fds initialized?
// if (_fd < 0) {
//         // open fd
//         _fd = ::open(_port, O_RDWR | O_NOCTTY);
// }
if(_fd<0)
{
        _fd = ::open((char *)"/dev/ttyS1", O_RDWR | O_NOCTTY);

        if (_fd < 0)
        {
            PX4_ERR("Error opening fd");
            //return;
        }

        // baudrate 115200, 8 bits, no parity, 1 stop bit
        unsigned speed = B9600;
        termios uart_config{};
        int termios_state{};

        tcgetattr(_fd, &uart_config);

        // clear ONLCR flag (which appends a CR for every LF)
        uart_config.c_oflag &= ~ONLCR;

        // set baud rate
        if ((termios_state = cfsetispeed(&uart_config, speed)) < 0)
        {
            PX4_ERR("CFG: %d ISPD", termios_state);
        }

        if ((termios_state = cfsetospeed(&uart_config, speed)) < 0)
        {
            PX4_ERR("CFG: %d OSPD\n", termios_state);
        }

        if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0)
        {
            PX4_ERR("baud %d ATTR", termios_state);
        }

        uart_config.c_cflag |= (CLOCAL | CREAD);	// ignore modem controls
        uart_config.c_cflag &= ~CSIZE;
        uart_config.c_cflag |= CS8;			// 8-bit characters
        uart_config.c_cflag &= ~PARENB;			// no parity bit
        uart_config.c_cflag &= ~CSTOPB;			// only need 1 stop bit
        uart_config.c_cflag &= ~CRTSCTS;		// no hardware flowcontrol

        // setup for non-canonical mode
        uart_config.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
        uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
        uart_config.c_oflag &= ~OPOST;

        // fetch bytes as they become available
        uart_config.c_cc[VMIN] = 1;
        uart_config.c_cc[VTIME] = 1;
}
collect();
}

int SerialNode::task_spawn(int argc, char *argv[])
{
	SerialNode *instance = new SerialNode();

	if (instance) {
		_object.store(instance);
		_task_id = task_id_is_work_queue;

		if (instance->init()) {
			return PX4_OK;
		}

	} else {
		PX4_ERR("alloc failed");
	}

	delete instance;
	_object.store(nullptr);
	_task_id = -1;

	return PX4_ERROR;
}

int SerialNode::print_status()
{
	perf_print_counter(_loop_perf);
	perf_print_counter(_loop_interval_perf);
	return 0;
}

int SerialNode::custom_command(int argc, char *argv[])
{
	return print_usage("unknown command");
}

int SerialNode::print_usage(const char *reason)
{
	if (reason) {
		PX4_WARN("%s\n", reason);
	}

	PRINT_MODULE_DESCRIPTION(
		R"DESCR_STR(
### Description
Example of a simple module running out of a work queue.

)DESCR_STR");

	PRINT_MODULE_USAGE_NAME("work_item_example", "template");
	PRINT_MODULE_USAGE_COMMAND("start");
	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();

	return 0;
}

extern "C" __EXPORT int serial_node_main(int argc, char *argv[])
{
	return SerialNode::main(argc, argv);
}
